﻿#include "PCLICP.h"
#include <vtkGenericOpenGLRenderWindow.h>
#include <vtkAxesActor.h>
#include <vtkOBJReader.h>
#include <pcl/io/pcd_io.h>            
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_io.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>  
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/common/transforms.h>
#include <pcl/surface/gp3.h>
#include <pcl/surface/poisson.h>
#include <pcl/surface/mls.h>          
#include <pcl/surface/marching_cubes_hoppe.h>
#include <pcl/surface/marching_cubes_rbf.h>
#include <pcl/registration/icp.h>
PCLICP::PCLICP(QWidget* parent)
	: QMainWindow(parent)
{
	vtkWidget = new QVTKOpenGLNativeWidget(this);
	this->resize(600, 400);
	this->setCentralWidget(vtkWidget);
	cloudIn.reset(new pcl::PointCloud<pcl::PointXYZ>);
	cloudIcp.reset(new pcl::PointCloud<pcl::PointXYZ>);
	cloudTr.reset(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPLYFile<pcl::PointXYZ>("D:/TEST/3D/ply/right.ply", *cloudIn);

	// 平移为cloudTr
	pcl::transformPointCloud(*cloudIn,
		*cloudTr,
		Eigen::Affine3f(Eigen::Translation3f(5, 10, 15)));

	// x,y,z旋转
	pcl::transformPointCloud(*cloudTr,
		*cloudTr,
		Eigen::Affine3f(Eigen::AngleAxisf(M_PI / 8, Eigen::Vector3f::UnitX())) *
		Eigen::Affine3f(Eigen::AngleAxisf(M_PI / 8, Eigen::Vector3f::UnitY())) *
		Eigen::Affine3f(Eigen::AngleAxisf(M_PI / 8, Eigen::Vector3f::UnitZ())));

	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	//设置要执行的初始迭代次数（默认值为1）
	icp.setMaximumIterations(100);
	//设置输入点云
	icp.setInputSource(cloudTr);
	//设置目标点云（输入点云进行仿射变换，得到目标点云）
	icp.setInputTarget(cloudIn);
	//匹配后源点云
	icp.align(*cloudIcp);
	//输出变换矩阵的适合性评估，检查ICP算法是否收敛；
	if (icp.hasConverged())
	{
		qDebug() << "\nICP has converged, score is "
			<< icp.getFitnessScore();
		//auto matrix = icp.getFinalTransformation().cast<double>();
	}
	// PCL 点云显示到VTK
	auto renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
	auto renderer = vtkSmartPointer<vtkRenderer>::New();
	viewer = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer(
		renderer
		, renderWindow, "", false));
	viewer->addPointCloud<pcl::PointXYZ>(cloudIn, "cloudIn");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 1, "cloudIn");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloudIn");

	viewer->addPointCloud<pcl::PointXYZ>(cloudIcp, "cloudIcp");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "cloudIcp");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloudIcp");

	viewer->addPointCloud<pcl::PointXYZ>(cloudTr, "cloudTr");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "cloudTr");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloudTr");
	// VTK 点云显示到VTK
	vtkWidget->setRenderWindow(viewer->getRenderWindow());
	viewer->setupInteractor(vtkWidget->interactor(), vtkWidget->renderWindow());
}

PCLICP::~PCLICP()
{}
